/*
 * CAM.c
 *
 *  Created on: 2023楠烇拷1閺堬拷18閺冿拷
 *      Author: 0
 */
#include "zf_common_headfile.h"

// 迷宫法爬线所用
/* dir存储的相对方向
 *   0
 * 3   1
 *   2
 */
const int dir_front[4][2] = {{0, -1}, {1, 0}, {0, 1}, {-1, 0}};
/* dir_frontleft存储的相对方向
 * 0   1
 *
 * 3   2
 */
const int dir_frontleft[4][2] = {{-1, -1}, {1, -1}, {1, 1}, {-1, 1}};
/* dir_frontleft存储的相对方向
 * 3   0
 *
 * 2   1
 */
const int dir_frontright[4][2] = {{1, -1}, {1, 1}, {-1, 1}, {-1, -1}};

// 摄像头图像信息结构体
image_t img_raw = DEF_IMAGE(NULL, MT9V03X_W, MT9V03X_H);

float thres = 0; // 二值化阈值

float block_size = 2; // 图像边界
float clip_value = 2; //

// 爬线时找边线起始点所用xy
float begin_x = 5;
float begin_y = 70;//55;

float line_blur_kernel = 7; // 滤波使用

float pixel_per_meter = 100 * 2 / 3; // 图像大小与真实大小对应关系

float sample_dist = 0.02;   // 等距采样距离
float angle_dist = 0.2;     // 角度采样距离

float aim_distance = 0;//= 0.68;    // 预瞄点

int ipts0[POINTS_MAX_LEN][2]; // 在原图上左线的坐标
int ipts1[POINTS_MAX_LEN][2]; // 在原图上右线的坐标
int ipts0_num = 0, ipts1_num = 0;

float rpts0[POINTS_MAX_LEN][2];  // 逆透视后的左线坐标
float rpts1[POINTS_MAX_LEN][2];  // 逆透视后的右线坐标
int rpts0_num = 0, rpts1_num = 0;

float rpts0b[POINTS_MAX_LEN][2];  // 滤波后的逆透视左线坐标
float rpts1b[POINTS_MAX_LEN][2];  // 滤波后的逆透视右线坐标
int rpts0b_num = 0, rpts1b_num = 0;

float rpts0s[POINTS_MAX_LEN][2];  // 等距采样后的滤波逆透视左线坐标
float rpts1s[POINTS_MAX_LEN][2];  // 等距采样后的滤波逆透视右线坐标
int rpts0s_num = 0, rpts1s_num = 0;

float rpts0a[POINTS_MAX_LEN];  // 左线局部角变化率,弧度
float rpts1a[POINTS_MAX_LEN];  // 右线局部角变化率,弧度
int rpts0a_num = 0, rpts1a_num = 0;

float rpts0an[POINTS_MAX_LEN];  // 左线角变化率非极大抑制
float rpts1an[POINTS_MAX_LEN];  // 右线角变化率非极大抑制
int rpts0an_num = 0, rpts1an_num = 0;

float rptsc0[POINTS_MAX_LEN][2];  // 左中线
float rptsc1[POINTS_MAX_LEN][2];  // 右中线
int rptsc0_num = 0, rptsc1_num = 0;

// 指向要循线的边线
float (*rpts)[2] = NULL;
int rpts_num;

// 预瞄点所在数组
float rptsn[POINTS_MAX_LEN][2];
int rptsn_num = 0;

// 记录左右边线拐弯点在数组中的下标
int Lpt0_rpts0s_id_left = 0, Lpt0_rpts0s_id_right = 0;
int Lpt1_rpts1s_id_left = 0, Lpt1_rpts1s_id_right = 0;

// 左右边线拐弯方向
bool Lpt0_found_left = false, Lpt0_found_right = false;
bool Lpt1_found_left = false, Lpt1_found_right = false;

// 左右边线是否为直道
bool is_straight0 = false, is_straight1 = false;
bool is_bend0 = false, is_bend1 = false;

// 预瞄点在中线位置
int aim_idx = 0;

// 记录循线方向标志位
enum track_type_e track_type = TRACK_RIGHT;

const char* track_type_name[] = {
            "TRACK_LEFT        ",
            "TRACK_RIGHT       ",
            "TRACK_CROSS_LEFT  ",
            "TRACK_CROSS_RIGHT ",
            "TRACK_CIRCLE_LEFT ",
            "TRACK_CIRCLE_RIGHT",
            "TRACK_GARAGE_LEFT ",
            "TRACK_GARAGE_RIGHT"
};

void cam (void)
{
    // 目标中心点
    float cx, cy;

    // 获取图像,将摄像头二维数组转为一维
    img_raw.data = mt9v03x_image[0];

    // 大津法获取二值化阈值
    thres = GetOSTU(mt9v03x_image);

    // 出赛道停车
    if (AT_IMAGE(&img_raw, 50, 90) < thres && AT_IMAGE(&img_raw, 140, 90) < thres)
        protect_flag = 0;

    // 对图像进行处理,获取左右边线
    process_image();

    // 赛道类型判别

    // 对左右边线进行检测,判断形状
    find_corners_polarity();

    // 检测前方是否为弯道
    check_bend();

    // 检测斑马线停车
    check_stop_car();

    // 检测前方是否为坡道
    check_rampway();

    // 判断是否需要检查前方有无十字路口
    check_cross();

    // 判断是否需要检查前方有无圆环
    check_circle();

    if (JOIN_OTHER_STATE)
    {
        // 根据处理后的边线判断循左右哪条边线
//        if (rptsc0_num > round(0.3 / sample_dist))
//            track_type = TRACK_LEFT;
//        else if (rptsc0_num < rptsc1_num / 2)
//            track_type = TRACK_RIGHT;
//        else if (rptsc0_num < 20 && rptsc1_num > rptsc0_num)
//            track_type = TRACK_RIGHT;
//
//        if (rptsc1_num > round(0.3 / sample_dist))
//            track_type = TRACK_RIGHT;
//        else if (rptsc1_num < rptsc0_num / 2)
//            track_type = TRACK_LEFT;
//        else if (rptsc1_num < 20 && rptsc0_num > rptsc1_num)
//            track_type = TRACK_LEFT;

        if (rptsc0_num < rptsc1_num)
            track_type = TRACK_RIGHT;
        else
            track_type = TRACK_LEFT;
    }

    // 跑十字
    run_cross();

    // 跑圆环
    run_circle();

    // 调整rpts指向边线数组
    if (track_type == TRACK_LEFT)
    {
        rpts = rptsc0;
        rpts_num = rptsc0_num;
    }
    else if (track_type == TRACK_RIGHT)
    {
        rpts = rptsc1;
        rpts_num = rptsc1_num;
    }
    else if (track_type == TRACK_CIRCLE_LEFT)
    {
        rpts = crptsc0;
        rpts_num = crptsc0_num;
    }
    else if (track_type == TRACK_CIRCLE_RIGHT)
    {
        rpts = crptsc1;
        rpts_num = crptsc1_num;
    }
    else if (track_type == TRACK_CROSS_LEFT)
    {
        rpts = far_rptsc0;
        rpts_num = far_rptsc0_num;
    }
    else if (track_type == TRACK_CROSS_RIGHT)
    {
        rpts = far_rptsc1;
        rpts_num = far_rptsc1_num;
    }

    // 控制速度以及预瞄点
    if (circle_type != CIRCLE_NONE)
    {
        if (circle_type == CIRCLE_RIGHT_RUNNING || circle_type == CIRCLE_LEFT_RUNNING)
        {
            aim_distance = HALF_ADJUST(aim_distance, circle_aim_distance_flash);
            set_speed = HALF_ADJUST(set_speed, circle_set_speed_flash);
        }
        else
        {
            aim_distance = HALF_ADJUST(aim_distance, bend_aim_distance_flash);
            set_speed = HALF_ADJUST(set_speed, bend_set_speed_flash);
        }
    }
    else if (cross_type != CROSS_NONE)
    {
        aim_distance = HALF_ADJUST(aim_distance, cross_aim_distance_flash);
        set_speed = HALF_ADJUST(set_speed, cross_set_speed_flash);
    }
    else if (stop_car_type != STOP_NONE)
    {
        aim_distance = HALF_ADJUST(aim_distance, aim_distance_flash);
        set_speed = HALF_ADJUST(set_speed, set_speed);
    }
    else if (is_bend0 == true || is_bend1 == true)
    {
        aim_distance = HALF_ADJUST(aim_distance, bend_aim_distance_flash);
        set_speed = HALF_ADJUST(set_speed, bend_set_speed_flash);
    }
    else if (is_straight0 && is_straight1)
    {
        aim_distance = HALF_ADJUST(aim_distance, aim_distance_flash);
        set_speed = HALF_ADJUST(set_speed, set_speed_flash);
    }

    // 找到距离中心点最近的一个中线坐标的数组下标
    cx = RESULT_COL / 2;
    cy = RESULT_ROW - 2;
    float min_dist = 1e10;
    int begin_id = -1;
    for (int i = 0; i < rpts_num; i++)
    {
        float dx = rpts[i][0] - cx;
        float dy = rpts[i][1] - cy;
        float dist = sqrt(dx * dx + dy * dy);
        if (dist < min_dist)
        {
            min_dist = dist;
            begin_id = i;
        }
    }

    // 判断该点坐标是否符合要求
    if (begin_id >= 0 && rpts_num - begin_id >= 3)
    {
        // 将该点的坐标置为中心点
        rpts[begin_id][0] = cx;
        rpts[begin_id][1] = cy;
        // 对中线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
        rptsn_num = sizeof(rptsn) / sizeof(rptsn[0]);
        resample_points(rpts + begin_id, rpts_num - begin_id, rptsn, &rptsn_num, sample_dist * pixel_per_meter);

        // 找到预瞄点在数组中的下标
        aim_idx = clip(round(aim_distance / sample_dist), 0, rptsn_num - 1);

        // 计算预瞄点和中心点的差值
        float dx = rptsn[aim_idx][0] - cx;
        float dy = cy - rptsn[aim_idx][1];// + circle_aim_distance_flash;

        // 得到误差
        float error = -atan2f(dx, dy) * 180 / PI;
        // 计算pid
        servo_duty = pid_solve_nom(&servo_pid, error);
    }
    else
    {
        // 否则就进入下一次计算
        rptsn_num = 0;
    }
}

// 找左线的起始点,找到的起始点为白色像素
void find_first_line_l (int *x1, int *y1, int half, int *num, int y_block)
{
    int temp = (*x1);

    for (; (*y1) > half + y_block; (*y1) -= 2)
    {
        for ((*x1) = temp; (*x1) - 1 > half; (*x1)--)
        {
            // 查看左侧像素是否为黑色
            if (AT_IMAGE(&img_raw, (*x1) - 1, (*y1)) < thres)
                return;
        }
    }
    *num = 0;
}

// 找右线的起始点,找到的起始点为白色像素
void find_first_line_r (int *x2, int *y2, int half, int *num, int y_block)
{
    int temp = (*x2);

    for (; (*y2) > half + y_block; (*y2) -= 2)
    {
        for ((*x2) = temp; (*x2) + 1 < img_raw.width - 1 - half; (*x2)++)
        {
            // 查看左侧像素是否为黑色
            if (AT_IMAGE(&img_raw, (*x2) + 1, (*y2)) < thres)
                return;
        }
    }
    *num = 0;
}

// 处理图像,获取左右边线
void process_image (void)
{
    uint8 value = 0;
    int x1, y1, x2, y2;

    ipts0_num = sizeof(ipts0) / sizeof(ipts0[0]);    //ipts0_num = POINTS_MAX_LEN
    // 找左线的起始白色像素点
    x1 = img_raw.width / 2 - begin_x, y1 = begin_y;
    find_first_line_l(&x1, &y1, block_size, &ipts0_num, 20);
    // 迷宫法爬左线
    findline_lefthand_adaptive(&img_raw, block_size, thres, x1, y1, ipts0, &ipts0_num, 0);

    // 若爬到的最后一个点的y值大于第一个点的y值,认为数组中数据无效
    if (ipts0[ipts0_num - 1][1] > ipts0[0][1])
        ipts0_num = 0;

    ipts1_num = sizeof(ipts1) / sizeof(ipts1[0]);
    // 找右线的起始白色像素点
    x2 = img_raw.width / 2 + begin_x, y2 = begin_y;
    find_first_line_r(&x2, &y2, block_size, &ipts1_num, 20);
    // 迷宫法爬右线
    findline_righthand_adaptive(&img_raw, block_size, thres, x2, y2, ipts1, &ipts1_num, 0);

    // 若爬到的最后一个点的y值大于第一个点的y值,认为数组中数据无效
    if (ipts1[ipts1_num - 1][1] > ipts1[0][1])
        ipts1_num = 0;

    // 对左线进行进行逆透视变换,将爬到的在原图的坐标转换为在逆透视图上的坐标
    if (ipts0_num != 0)
    {
        for (int i = 0; i < ipts0_num; i++)
        {
            value = ImagePerspective(ipts0[i][0], ipts0[i][1], &rpts0[i][0], &rpts0[i][1]);
            if (value == 1)
                break;
            rpts0_num = i;
        }
    }
    else
        rpts0_num = 0;

    // 对右线进行进行逆透视变换,将爬到的在原图的坐标转换为在逆透视图上的坐标
    if (ipts1_num != 0)
    {
        for (int i = 0; i < ipts1_num; i++)
        {
            value = ImagePerspective(ipts1[i][0], ipts1[i][1], &rpts1[i][0], &rpts1[i][1]);
            if (value == 1)
                break;
            rpts1_num = i;
        }
    }
    else
        rpts1_num = 0;

    // 对左右边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
    blur_points(rpts0, rpts0_num, rpts0b, (int) round(line_blur_kernel));
    rpts0b_num = rpts0_num;
    blur_points(rpts1, rpts1_num, rpts1b, (int) round(line_blur_kernel));
    rpts1b_num = rpts1_num;

    // 对左右边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
    rpts0s_num = sizeof(rpts0s) / sizeof(rpts0s[0]);
    resample_points2(rpts0b, rpts0b_num, rpts0s, &rpts0s_num, sample_dist * pixel_per_meter);
    rpts1s_num = sizeof(rpts1s) / sizeof(rpts1s[0]);
    resample_points2(rpts1b, rpts1b_num, rpts1s, &rpts1s_num, sample_dist * pixel_per_meter);

    // 计算左右边线的局部角变化率
    local_angle_points(rpts0s, rpts0s_num, rpts0a, (int) round(angle_dist / sample_dist));
    rpts0a_num = rpts0s_num;
    local_angle_points(rpts1s, rpts1s_num, rpts1a, (int) round(angle_dist / sample_dist));
    rpts1a_num = rpts1s_num;

    // 计算左右边线的局部角变化率非极大抑制
    nms_angle(rpts0a, rpts0a_num, rpts0an, (int) round(angle_dist / sample_dist) * 2 + 1);
    rpts0an_num = rpts0a_num;
    nms_angle(rpts1a, rpts1a_num, rpts1an, (int) round(angle_dist / sample_dist) * 2 + 1);
    rpts1an_num = rpts1a_num;

    // 根据左右边线分别计算左右中线
    track_leftline(rpts0s, rpts0s_num, rptsc0, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
    rptsc0_num = rpts0s_num;
    track_rightline(rpts1s, rpts1s_num, rptsc1, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
    rptsc1_num = rpts1s_num;
}

// 迷宫法爬左线,白线
void findline_lefthand_adaptive (image_t *img, int half, int clip_value, int x, int y, int pts[][2], int *num, int step_num)
{
    int step = step_num, dir = 0, turn = 0;

    while (step < *num && half < x && x < img->width - 1 - half && half < y && y < img->height - half && turn < 4)
    {
        int local_thres = clip_value;

        // 获取上方和左上方的灰度值
        int front_value = AT(img, x + dir_front[dir][0], y + dir_front[dir][1]);
        int frontleft_value = AT(img, x + dir_frontleft[dir][0], y + dir_frontleft[dir][1]);

        if (front_value < local_thres)
        {
            // 上方像素为黑色,向右转向
            dir = (dir + 1) % 4;
            turn++;
        }
        else if (frontleft_value < local_thres)
        {
            // 左上方像素为黑色,上方像素为白色,向前走
            x += dir_front[dir][0];
            y += dir_front[dir][1];
            // 存入数组
            pts[step][0] = x;
            pts[step][1] = y;
            step++;
            turn = 0;
        }
        else
        {
            // 左上方像素为白色,上方像素为白色,向左上走
            x += dir_frontleft[dir][0];
            y += dir_frontleft[dir][1];
            // 右转
            dir = (dir + 3) % 4;
            pts[step][0] = x;
            pts[step][1] = y;
            step++;
            turn = 0;
        }
    }
    // 记录长度
    *num = step;
}

// 迷宫法爬右线,白线
void findline_righthand_adaptive (image_t *img, int half, int clip_value, int x, int y, int pts[][2], int *num, int step_num)
{
    int step = step_num, dir = 0, turn = 0;

    while (step < *num && half < x && x < img->width - 1 - half && half < y && y < img->height - half && turn < 4)
    {
        int local_thres = clip_value;

        // 获取上方和右上方的灰度值
        int front_value = AT(img, x + dir_front[dir][0], y + dir_front[dir][1]);
        int frontright_value = AT(img, x + dir_frontright[dir][0], y + dir_frontright[dir][1]);
        if (front_value < local_thres)
        {
            // 上方像素为黑色,向左转向
            dir = (dir + 3) % 4;
            turn++;
        }
        else if (frontright_value < local_thres)
        {
            // 右上方像素为黑色,上方像素为白色,向前走
            x += dir_front[dir][0];
            y += dir_front[dir][1];
            pts[step][0] = x;
            pts[step][1] = y;
            step++;
            turn = 0;
        }
        else
        {
            // 右上方像素为白色,上方像素为白色,向右上走
            x += dir_frontright[dir][0];
            y += dir_frontright[dir][1];
            // 左转
            dir = (dir + 1) % 4;
            pts[step][0] = x;
            pts[step][1] = y;
            step++;
            turn = 0;
        }
    }
    // 存入数组长度
    *num = step;
}

// 对边线三角滤波
void blur_points (float pts_in[][2], int num, float pts_out[][2], int kernel)
{
    int half = kernel / 2;
    for (int i = 0; i < num; i++)
    {
        pts_out[i][0] = pts_out[i][1] = 0;
        for (int j = -half; j <= half; j++)
        {
            pts_out[i][0] += pts_in[clip(i + j, 0, num - 1)][0] * (half + 1 - abs(j));
            pts_out[i][1] += pts_in[clip(i + j, 0, num - 1)][1] * (half + 1 - abs(j));
        }
        pts_out[i][0] /= (2 * half + 2) * (half + 1) / 2;
        pts_out[i][1] /= (2 * half + 2) * (half + 1) / 2;
    }
}

// 对边线等距采样
void resample_points (float pts_in[][2], int num1, float pts_out[][2], int *num2, float dist)
{
    int remain = 0, len = 0;
    for (int i = 0; i < num1 - 1 && len < *num2; i++)
    {
        float x0 = pts_in[i][0];
        float y0 = pts_in[i][1];
        float dx = pts_in[i + 1][0] - x0;
        float dy = pts_in[i + 1][1] - y0;
        float dn = sqrt(dx * dx + dy * dy);
        dx /= dn;
        dy /= dn;

        while (remain < dn && len < *num2)
        {
            x0 += dx * remain;
            pts_out[len][0] = x0;
            y0 += dy * remain;
            pts_out[len][1] = y0;

            len++;
            dn -= remain;
            remain = dist;
        }
        remain -= dn;
    }
    *num2 = len;
}

// 对边线等距采样
// TODO: fix bug
void resample_points2 (float pts_in[][2], int num1, float pts_out[][2], int *num2, float dist)
{
    if (num1 < 0)
    {
        *num2 = 0;
        return;
    }
    pts_out[0][0] = pts_in[0][0];
    pts_out[0][1] = pts_in[0][1];
    int len = 1;
    for (int i = 0; i < num1 - 1 && len < *num2; i++)
    {
        float x0 = pts_in[i][0];
        float y0 = pts_in[i][1];
        float x1 = pts_in[i + 1][0];
        float y1 = pts_in[i + 1][1];

        do
        {
            float x = pts_out[len - 1][0];
            float y = pts_out[len - 1][1];

            float dx0 = x0 - x;
            float dy0 = y0 - y;
            float dx1 = x1 - x;
            float dy1 = y1 - y;

            float dist0 = sqrt(dx0 * dx0 + dy0 * dy0);
            float dist1 = sqrt(dx1 * dx1 + dy1 * dy1);

            float r0 = (dist1 - dist) / (dist1 - dist0);
            float r1 = 1 - r0;

            if (r0 < 0 || r1 < 0)
                break;
            x0 = x0 * r0 + x1 * r1;
            y0 = y0 * r0 + y1 * r1;
            pts_out[len][0] = x0;
            pts_out[len][1] = y0;
            len++;
        }while (len < *num2);

    }
    *num2 = len;
}

// 处理边线局部角变化率
void local_angle_points (float pts_in[][2], int num, float angle_out[], int dist)
{
    for (int i = 0; i < num; i++)
    {
        if (i <= 0 || i >= num - 1)
        {
            angle_out[i] = 0;
            continue;
        }
        float dx1 = pts_in[i][0] - pts_in[clip(i - dist, 0, num - 1)][0];
        float dy1 = pts_in[i][1] - pts_in[clip(i - dist, 0, num - 1)][1];
        float dn1 = sqrtf(dx1 * dx1 + dy1 * dy1);
        float dx2 = pts_in[clip(i + dist, 0, num - 1)][0] - pts_in[i][0];
        float dy2 = pts_in[clip(i + dist, 0, num - 1)][1] - pts_in[i][1];
        float dn2 = sqrtf(dx2 * dx2 + dy2 * dy2);
        float c1 = dx1 / dn1;
        float s1 = dy1 / dn1;
        float c2 = dx2 / dn2;
        float s2 = dy2 / dn2;
        angle_out[i] = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);
    }
}

// 处理边线角变化率非极大抑制
void nms_angle (float angle_in[], int num, float angle_out[], int kernel)
{
    // zf_assert(kernel % 2 == 1);
    int half = kernel / 2;
    for (int i = 0; i < num; i++)
    {
        angle_out[i] = angle_in[i];
        for (int j = -half; j <= half; j++)
        {
            if (fabs(angle_in[clip(i + j, 0, num - 1)]) > fabs(angle_out[i]))
            {
                angle_out[i] = 0;
                break;
            }
        }
    }
}

// 计算左中线
void track_leftline (float pts_in[][2], int num, float pts_out[][2], int approx_num, float dist)
{
    for (int i = 0; i < num; i++)
    {
        float dx = pts_in[clip(i + approx_num, 0, num - 1)][0] - pts_in[clip(i - approx_num, 0, num - 1)][0];
        float dy = pts_in[clip(i + approx_num, 0, num - 1)][1] - pts_in[clip(i - approx_num, 0, num - 1)][1];
        float dn = sqrt(dx * dx + dy * dy);
        dx /= dn;
        dy /= dn;
        pts_out[i][0] = pts_in[i][0] - dy * dist;
        pts_out[i][1] = pts_in[i][1] + dx * dist;
    }
}

// 计算右中线
void track_rightline (float pts_in[][2], int num, float pts_out[][2], int approx_num, float dist)
{
    for (int i = 0; i < num; i++)
    {
        float dx = pts_in[clip(i + approx_num, 0, num - 1)][0] - pts_in[clip(i - approx_num, 0, num - 1)][0];
        float dy = pts_in[clip(i + approx_num, 0, num - 1)][1] - pts_in[clip(i - approx_num, 0, num - 1)][1];
        float dn = sqrt(dx * dx + dy * dy);
        dx /= dn;
        dy /= dn;
        pts_out[i][0] = pts_in[i][0] + dy * dist;
        pts_out[i][1] = pts_in[i][1] - dx * dist;
    }
}

// 取中间值(int)
int clip (int x, int low, int up)
{
    return x > up ? up : x < low ? low : x;
}
// 取中间值(float)
float fclip (float x, float low, float up)
{
    return x > up ? up : x < low ? low : x;
}

// 判断左右边线是否为直道,若不是则找出拐弯方向
void find_corners_polarity (void)
{
    Lpt0_found_left = Lpt0_found_right = Lpt1_found_left = Lpt1_found_right = false;
    // 判断左右边线长度是否足够
    is_straight0 = rpts0s_num > 0.6 / sample_dist;
    is_straight1 = rpts1s_num > 0.6 / sample_dist;

    // 处理左线
    for (int i = 0; i < rpts0a_num; i++)
    {
        // 找到该下标的角度与哪两个点组成的
        int im1 = clip(i - (int) round(angle_dist / sample_dist), 0, rpts0s_num - 1);
        int ip1 = clip(i + (int) round(angle_dist / sample_dist), 0, rpts0s_num - 1);

        float conf = (fabs(rpts0a[i]) - (fabs(rpts0a[im1]) + fabs(rpts0a[ip1])) / 2);

        if (rpts0a[i] < 0)
        {
            // 70 < 该点的角度 < 110,左边线向左拐
            if (Lpt0_found_left == false && 70. / 180. * PI < conf && conf < 130. / 180. * PI /*&& i < 0.4 / sample_dist*/)
            {
                // 记录该点在数组中的下标
                Lpt0_rpts0s_id_left = i;
                Lpt0_found_left = true;
            }
        }
        else if (rpts0a[i] > 0)
        {
            // 70 < 该点的角度 < 110,左边线向右拐
            if (Lpt0_found_right == false && 70. / 180. * PI < conf && conf < 130. / 180. * PI /*&& i < 0.4 / sample_dist*/)
            {
                Lpt0_rpts0s_id_right = i;
                Lpt0_found_right = true;
            }
        }

        // 若conf > 10则认为非直道
        if (is_straight0 == true && conf > 10. / 180. * PI /*&& i < 1.5 / sample_dist*/)
            is_straight0 = false;

        // 找到左拐和右拐且非直线就跳出
        if (Lpt0_found_left == true && Lpt0_found_right == true && is_straight0 == false)
            break;
    }

    // 处理右线
    for (int i = 0; i < rpts1a_num; i++)
    {
        int im1 = clip(i - (int) round(angle_dist / sample_dist), 0, rpts1s_num - 1);
        int ip1 = clip(i + (int) round(angle_dist / sample_dist), 0, rpts1s_num - 1);
        float conf = (fabs(rpts1a[i]) - (fabs(rpts1a[im1]) + fabs(rpts1a[ip1])) / 2);
        if (rpts1a[i] < 0)
        {
            // 70 < 该点的角度 < 110,右边线向左拐
            if (Lpt1_found_left == false && 70. / 180. * PI < conf && conf < 130. / 180. * PI /*&& i < 0.4 / sample_dist*/)
            {
                Lpt1_rpts1s_id_left = i;
                Lpt1_found_left = true;
            }
        }
        else if (rpts1a[i] > 0)
        {
            // 70 < 该点的角度 < 110,右边线向右拐
            if (Lpt1_found_right == false && 70. / 180. * PI < conf && conf < 130. / 180. * PI /*&& i < 0.4 / sample_dist*/)
            {
                Lpt1_rpts1s_id_right = i;
                Lpt1_found_right = true;
            }
        }

        // 若conf > 10则认为非直道
        if (is_straight1 == true && conf > 10. / 180. * PI /*&& i < 1.5 / sample_dist*/)
            is_straight1 = false;

        // 找到左拐和右拐且非直线就跳出
        if (Lpt1_found_left == true && Lpt1_found_right == true && is_straight1 == false)
            break;
    }
}

// 检测弯道
void check_bend()
{
    is_bend0 = is_bend1 = false;
    for (int i = 0; i < clip(bend_check_aim_distance / sample_dist, 0, rpts0a_num); i++)
    {
        int im1 = clip(i - (int) round(angle_dist / sample_dist), 0, rpts0s_num - 1);
        int ip1 = clip(i + (int) round(angle_dist / sample_dist), 0, rpts0s_num - 1);

        float conf = (fabs(rpts0a[i]) - (fabs(rpts0a[im1]) + fabs(rpts0a[ip1])) / 2);
        if (is_bend0 == false && conf > 8. / 180. * PI && conf < 80. / 180. * PI)
        {
            is_bend0 = true;
            break;
        }
    }
    for (int i = 0; i < clip(bend_check_aim_distance / sample_dist, 0, rpts1a_num); i++)
    {
        int im1 = clip(i - (int) round(angle_dist / sample_dist), 0, rpts1s_num - 1);
        int ip1 = clip(i + (int) round(angle_dist / sample_dist), 0, rpts1s_num - 1);

        float conf = (fabs(rpts1a[i]) - (fabs(rpts1a[im1]) + fabs(rpts1a[ip1])) / 2);
        if (is_bend1 == false && conf > 8. / 180. * PI && conf < 80. / 180. * PI)
        {
            is_bend1 = true;
            break;
        }
    }
}

// 大津法计算二值化阈值
uint16 GetOSTU (uint8 tmImage[][MT9V03X_W])
{
    signed short i, j;
    unsigned long Amount = 0;
    unsigned long PixelBack = 0;
    unsigned long PixelshortegralBack = 0;
    unsigned long Pixelshortegral = 0;
    signed long PixelshortegralFore = 0;
    signed long PixelFore = 0;
    float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma;
    signed short MinValue, MaxValue;
    signed short Threshold = 0;
    uint16 HistoGram[256];

    for (j = 0; j < 256; j++)
        HistoGram[j] = 0;

    for (j = 0; j < MT9V03X_H; j++)
    {
        for (i = 0; i < MT9V03X_W; i++)
        {
            HistoGram[tmImage[j][i]]++;
        }
    }

    for (MinValue = 0; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++)
        ;
    for (MaxValue = 255; MaxValue > MinValue && HistoGram[MinValue] == 0; MaxValue--)
        ;

    if (MaxValue == MinValue)
        return (unsigned char) MaxValue;
    if (MinValue + 1 == MaxValue)
        return (unsigned char) MinValue;
    for (j = MinValue; j <= MaxValue; j++)
        Amount += HistoGram[j];

    Pixelshortegral = 0;
    for (j = MinValue; j <= MaxValue; j++)
    {
        Pixelshortegral += HistoGram[j] * j;
    }
    SigmaB = -1;
    for (j = MinValue; j < MaxValue; j++)
    {
        PixelBack = PixelBack + HistoGram[j];
        PixelFore = Amount - PixelBack;
        OmegaBack = (float) PixelBack / Amount;
        OmegaFore = (float) PixelFore / Amount;
        PixelshortegralBack += HistoGram[j] * j;
        PixelshortegralFore = Pixelshortegral - PixelshortegralBack;
        MicroBack = (float) PixelshortegralBack / PixelBack;
        MicroFore = (float) PixelshortegralFore / PixelFore;
        Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore);
        if (Sigma > SigmaB)
        {
            SigmaB = Sigma;
            Threshold = j;
        }
    }
    return Threshold;
}

uint16 dim_GetOSTU (uint8 tmImage[][MT9V03X_W])
{
    //鍩烘湰鍙橀噺
    uint16 GrayScale = 255, N = 0;
    uint16 HistoGram[256];
    uint16 threshold = 0;
    //妯＄硦鍙橀噺
    uint16 L1 = 130, L2 = 150, L3 = 0, L4 = 240;
    float uA[150];
    float uB[240];
    float uC[255];
    float PA = 0, PB = 0, PC = 0;
    float PAA[150];
    float PBB[240];
    float PCC[255];
    float mA = 0, mB = 0, mC = 0;
    float S = 0, preS = 0;
    uint16 gray1 = 0, gray2 = 0;
    uint16 temp_point[255];
    uint16 temp_k = 0, temp_i = 0, Break_Point = 0;
    uint16 temp1 = 0, temp2 = 0;
    uint16 i = 0, j = 0;
    N = MT9V03X_H * MT9V03X_W;
//鏁扮粍娓呴浂
    for (j = 0; j < 255; j++)
        temp_point[j] = 0;

    for (j = 0; j < 255; j++)
        HistoGram[j] = 0;

    for (j = 0; j < 150; j++)
        uA[j] = 0;

    for (j = 0; j < 240; j++)
        uB[j] = 0;

    for (j = 0; j < 255; j++)
        uC[j] = 0;

    for (j = 0; j < 150; j++)
        PAA[j] = 0;

    for (j = 0; j < 240; j++)
        PBB[j] = 0;

    for (j = 0; j < 255; j++)
        PCC[j] = 0;

    //妯＄硦OSTU姹傞槇鍊�

    //1.缁熻鐏板害鐩存柟鍥�
    for (j = 0; j < MT9V03X_H; j++)
    {
        for (i = 0; i < MT9V03X_W; i++)
        {
            HistoGram[tmImage[j][i]]++;
        }
    }

    //2.瀵绘壘鍚堥�傜殑鍒嗗壊鐐�
    for (i = 1; i < GrayScale; i++)            //鍒嗙鐩存柟鍥捐疆寤�
    {
        if (HistoGram[i] >= HistoGram[i - 1])
            temp_point[i] = temp_point[i - 1] + 1;
        else
            temp_point[i] = temp_point[i - 1] - 1;

    }

    for (i = 10; i < GrayScale - 10; i++)        //瀵绘壘璋风偣
    {
        if (temp_point[i] < temp_point[i + 10] && temp_point[i] < temp_point[i - 10])
        {
            temp_i = temp_i + i;
            temp_k = temp_k + 1;
        }
    }

    Break_Point = (uint16) (temp_i / temp_k);

    //3.瀵绘壘伪鍖哄煙鍜屛插尯鍩熺殑鏈�楂橀鐜囩伆搴﹀��
    for (i = 0; i < Break_Point; i++)
    {
        if (temp1 < HistoGram[i])
        {
            temp1 = HistoGram[i];
            gray1 = i;
        }
    }

    for (i = Break_Point; i < 256; i++)
    {
        if (temp2 < HistoGram[i])
        {
            temp2 = HistoGram[i];
            gray2 = i;
        }
    }

    //4.閲嶆柊瀹氫箟鍖哄煙闂ㄩ檺
    L1 = gray1;
    L2 = gray1 + 30;
    L4 = gray2;
//    ips200_show_float(50, 1, L1, 7, 3);
//    ips200_show_float(50, 2, L2, 7, 3);
//    ips200_show_float(50, 3, L4, 7, 3);
    if (L4 <= L2)
        L4 = L2 + 20;

    //5.姹侫绫诲尯鍩熺伆搴﹀�肩浉鍏冲彉閲�
    for (i = 0; i < L2; i++)
    {
        if (i <= L1)
            uA[i] = 1.;
        else if (i > L1 && i <= L2)
            uA[i] = (float) ((i / (L1 - L2)) - (L2 / (L1 - L2)));

        PA = (float) (PA + uA[i] * HistoGram[i]);
    }

    for (i = 0; i < L2; i++)
    {
        PAA[i] = (float) (uA[i] * HistoGram[i] / PA);
        mA = (float) (mA + i * PAA[i]);
    }

    PA = PA / N;

    //6.婊戝姩L3姹傞槇鍊�
    for (L3 = L2; L3 < L4; L3++)
    {

        mB = 0;
        mC = 0;
        PB = 0;
        PC = 0;

        for (i = L1; i < L4; i++)          //姹侭绫诲尯鍩熺伆搴﹀�肩殑闅跺睘搴�
        {
            if (i >= L1 && i < L2)
                uB[i] = (float) ((i / (L2 - L1)) - (L1 / (L2 - L1)));
            else if (i >= L2 && i <= L3)
                uB[i] = 1.;
            else if (i > L3 && i <= L4)
                uB[i] = (float) ((i / (L3 - L4)) - (L4 / (L3 - L4)));

            PB = (float) (PB + uB[i] * HistoGram[i]);
        }

        for (i = L3; i < GrayScale; i++)   //姹侰绫诲尯鍩熺伆搴﹀�肩殑闅跺睘搴�
        {
            if (i < L4 && i >= L3)
                uC[i] = (float) ((i / (L4 - L3)) - (L3 / (L4 - L3)));
            else if (i >= L4)
                uC[i] = 1.;

            PC = (float) (PC + uC[i] * HistoGram[i]);
        }

        for (i = L1; i < L4; i++)   //姹侭鍖哄煙鍚勭伆搴﹀�煎嚭鐜扮殑姒傜巼
        {
            PBB[i] = uB[i] * HistoGram[i] / PB;
            mB = (float) (mB + i * PBB[i]);
        }

        for (i = L3; i < GrayScale; i++)   //姹侰鍖哄煙鍚勭伆搴﹀�煎嚭鐜扮殑姒傜巼
        {
            PCC[i] = (float) (uC[i] * HistoGram[i] / PC);
            mC = (float) (mC + i * PCC[i]);
        }

        PB = PB / N;
        PC = PC / N;

        S = PA * PB * PC * (mC - mA) * (mC - mB) * (mB - mA);

        if (S > preS)
        {
            preS = S;
            // threshold = (L1 + L2 + L3) / 3;
            threshold = (L2 + L3) / 2;

        }
    }

    return threshold;

}
